﻿#include "HChineRobotDll.h"
#include <pcl\io\pcd_io.h>
#include <pcl\common\transforms.h>
#include <opencv2/core/eigen.hpp>
bool
Robot::camera2Robot(std::vector<cv::Point3f>& centerPoints,std::vector<cv::Mat>& coordinates,std::vector<LocationDH>& locations)
{
	for (int i = 0; i < centerPoints.size(); ++i)
	{
		cv::Mat Rx, Ry, Rz;
		if (coordinates[i].type() == CV_64FC1)
		{
			Rx = cv::Mat(3, 1, CV_64FC1);
			Ry = cv::Mat(3, 1, CV_64FC1);
			Rz = cv::Mat(3, 1, CV_64FC1);
			Rx = coordinates[i].row(0);
			Ry = coordinates[i].row(1);
			Rz = coordinates[i].row(2);

		}
		if (coordinates[i].type() == CV_32FC1)
		{
			Rx = cv::Mat(3, 1, CV_32FC1);
			Ry = cv::Mat(3, 1, CV_32FC1);
			Rz = cv::Mat(3, 1, CV_32FC1);
			Rx = coordinates[i].row(0);
			Ry = coordinates[i].row(1);
			Rz = coordinates[i].row(2);
		}
		//zitaibianhuan
		cv::Mat cam_rx, cam_ry, cam_rz;
		cam_rx = cv::Mat(1, 3, CV_32FC1);
		cam_ry = cv::Mat(1, 3, CV_32FC1);
		cam_rz = cv::Mat(1, 3, CV_32FC1);
		cam_rx.at<float>(0, 0) = 48.796;
		cam_rx.at<float>(0, 1) = 10.55;
		cam_rx.at<float>(0, 2) = 2.74;

		cam_ry.at<float>(0, 0) = -10.612;
		cam_ry.at<float>(0, 1) = 48.854;
		cam_ry.at<float>(0, 2) = 0.807;

		cam_rz.at<float>(0, 0) = 2.507;
		cam_rz.at<float>(0, 1) = 1.37;
		cam_rz.at<float>(0, 2) = -49.918;
		cv::Point3f cam_pt(0, 0, 0);
		cv::Mat rob_mat(3, 1, CV_32FC1);
		rob_mat.at<float>(0, 0) = 177.21*CV_PI/180;
		rob_mat.at<float>(1, 0) = -3.896*CV_PI/180;
		rob_mat.at<float>(2, 0) = -53.743*CV_PI/180;
		cv::Mat  Mc,Mr;
		Eigen::Matrix4f Rt;
		Rt = euler2Rt(cam_pt.x,cam_pt.y,cam_pt.z, rob_mat.at<float>(0, 0), rob_mat.at<float>(1, 0), rob_mat.at<float>(2, 0));
		cv::eigen2cv(Rt, Mr);
		xyzRXYZ2Mat(cam_pt, cam_rx, cam_ry, cam_rz, Mc);
		cv::Mat eye2hand_r = Mr*Mc.inv();
		

		cv::Mat transformM;
		xyzRXYZ2Mat(centerPoints[i], Rx, Ry, Rz, transformM);
		cv::Point3d temp;
		cv::Mat eye2handMat(4,4,CV_32FC1);
		cv::eigen2cv(eyetohand_mat_, eye2handMat);
		XYZtransform(centerPoints[i].x, centerPoints[i].y, centerPoints[i].z, temp.x, temp.y, temp.z, eye2handMat);
		cv::Mat objInBase = eye2hand_r*transformM;
		double X, Y, Z, RX, RY, RZ;
		Mat2pose(objInBase, X, Y, Z, RX, RY, RZ);
		/*RZ = RZ - 202;*/
		/*RZ += 83*CV_PI/180;
		if (RZ < -CV_PI)
		{
			RZ = RZ + CV_PI;
		}*/
		/*RZ -= 180;*/
		LocationDH tmp_loc;
		tmp_loc.x = static_cast<float>(temp.x);
		tmp_loc.y = static_cast<float>(temp.y);
		tmp_loc.z = static_cast<float>(temp.z);
		tmp_loc.a = static_cast<float>(RX);
		tmp_loc.b = static_cast<float>(RY);
		tmp_loc.c = static_cast<float>(RZ);
		locations.push_back(tmp_loc);
	}
	return true;
}

bool
Robot::camera2Robot(LocationDH camera_pt, LocationDH& robot_pt, int flag)
{
	Eigen::Matrix4f camera_Rt = euler2Rt(camera_pt.x, camera_pt.y, camera_pt.z, camera_pt.a, camera_pt.b, camera_pt.c);
	Eigen::Matrix4f robot_Rt;
	if (flag == 0)
	{
		robot_Rt = eyetohand_mat_*camera_Rt;
	}
	else if (flag == 1)
		robot_Rt = eyeinhand_mat_*camera_Rt;
	else
		return false;
	return rt2euler(robot_Rt, robot_pt.x, robot_pt.y, robot_pt.z, robot_pt.a, robot_pt.b, robot_pt.c);
}
bool
Robot::camera2Robot(std::vector<LocationDH> camera_pts, std::vector<LocationDH>& robot_pts, int flag)
{
	robot_pts.resize(camera_pts.size());
	for (int i = 0; i < camera_pts.size(); ++i)
		if(!camera2Robot(camera_pts[i], robot_pts[i],flag))
			return false;
	return true;
}

bool 
Robot::getDestLoc(std::vector<LocationDH> input_dest, std::vector<LocationDH>& output)
{
	return path_plan_.getDestLoc(input_ori_,input_dest,output);
}
bool
Robot::setEnvirCloud(PointCloud& input_cloud)
{
	return path_plan_.setEnvirCloud(input_cloud);
}
bool
Robot::setScanCloud(PointCloud& input_cloud)
{
	return path_plan_.setScanCloud(input_cloud);
}
bool
Robot::loadConfigFile(std::string file_path)
{
	std::string str_eyetohand_mat = file_path + "\\标定矩阵.csv";
	pcl::PointCloud<pcl::PointXYZ>::Ptr mesh(new pcl::PointCloud<pcl::PointXYZ>());
	std::string str_envir = file_path + "\\envir.pcd";
	pcl::io::loadPCDFile(str_envir, *mesh);
	setEnvirCloud(*mesh);
	cv::Mat read_mat;
	if (!readCSV(str_eyetohand_mat,read_mat))
		return false;
	if (read_mat.cols == 4 && read_mat.rows == 4)
	{
		for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j)
			eyetohand_mat_(i, j) = read_mat.at<float>(i, j);
	}
	else
		return false;
	return path_plan_.loadConfigFile(file_path);
}
